#include <iostream>
#include <cstdio>
#include <vector>
#include <opencv/cv.h>
#include <opencv/highgui.h>

using namespace std;
using namespace cv;

// ***************************************************
// Default Parameters from OpenCV Face Detect. Example
// ***************************************************
String cascadeName = "haarcascade_frontalface_alt.xml";
String nestedCascadeName = "haarcascade_eye_tree_eyeglasses.xml";
double scale = 1.3;

// ***********
// Color Table
// ***********
const static Scalar colors[] =
{
	CV_RGB(0,0,255),
	CV_RGB(0,128,255),
	CV_RGB(0,255,255),
	CV_RGB(0,255,0),
	CV_RGB(255,128,0),
	CV_RGB(255,255,0),
	CV_RGB(255,0,0),
	CV_RGB(255,0,255)
};


CvFont font;

void
init_font()
{
	cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, 1.0, 1.0, 0, 0, CV_AA);
}


void
write_on_image(IplImage *img, char *text, CvPoint position, CvScalar color)
{
	cvPutText(img, text, position, &font, color);
}


void
draw_faces(vector<Rect> faces, Mat &img)
{
	int i = 0;
	Point center;

	for(vector<Rect>::const_iterator r = faces.begin(); r != faces.end(); r++, i++)
	{
		Scalar color = colors[i % 8];
		Rect face_container = Rect(r->x * scale, r->y * scale, r->width * scale, r->height * scale);
		rectangle(img, face_container, color, 1, 1, 0);
	}
}


void
detect_and_draw(IplImage* img_in, CascadeClassifier &cascade, double scale, vector<Rect> &faces)
{
	double t = 0;

	Mat img = img_in;
	Mat smallImg(cvRound (img.rows / scale), cvRound(img.cols / scale), CV_8UC1);

	resize(img, smallImg, smallImg.size(), 0, 0, INTER_LINEAR);
	equalizeHist(smallImg, smallImg);

	t = (double) cvGetTickCount();

	cascade.detectMultiScale(
		smallImg, faces,
		1.1, 2, 0
		//|CV_HAAR_FIND_BIGGEST_OBJECT,
		//|CV_HAAR_DO_ROUGH_SEARCH,
		|CV_HAAR_SCALE_IMAGE,
		Size(30, 30)
	);

	draw_faces(faces, img);

	t = (double) cvGetTickCount() - t;
	printf("detection time = %g ms\n", t / ((double) cvGetTickFrequency() * 1000.));
}


int
main(int argc, const char** argv)
{
	vector<Rect> faces;
	CvCapture* capture = 0;
	IplImage *frame, *gray = 0, *frame_orig;
	CascadeClassifier cascade;

	capture = cvCaptureFromCAM(0);

	if (!capture)
		exit(printf("Error: capture initialization error. Please check if the camera is turned on\n"));


	if(!cascade.load(cascadeName))
		exit(printf("Error: cascade file '%s' not found!\n", cascadeName.c_str()));

	init_font();
	cvNamedWindow("result", 1);

	frame = 0;

	while(1)
	{
		frame_orig = cvQueryFrame(capture);
			
		if (frame == NULL)	
			frame = cvCreateImage(cvSize(frame_orig->width * 0.5, frame_orig->height * 0.5), IPL_DEPTH_8U, 3);		

		cvResize(frame_orig, frame);

		if (gray == 0)
			gray = cvCreateImage(cvSize(frame->width, frame->height), IPL_DEPTH_8U, 1);
			
		cvCvtColor(frame, gray, CV_BGR2GRAY);
		detect_and_draw(gray, cascade, scale, faces);

		cvShowImage("result", gray);

		if ((cvWaitKey(10) & 255) == 27)
			break;
	}

	cvReleaseCapture(&capture);
	cvDestroyWindow("result");

	return 0;
}

